import sensor,time,image
import pid
from pyb import LED,Pin,Servo,UART
sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_framesize(sensor.QQVGA)
# You can set windowing to reduce the search image.
#sensor.set_windowing(((640-80)//2, (480-60)//2, 80, 60))
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.skip_frames()
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
led_red=LED(1)
led_green=LED(2)
led_buld=LED(3)
led_rr=LED(4)
led_rr.on()
led_red.on()
led_green.on()
led_buld.on()
S1=Servo(1)
S2=Servo(2)

uart=UART(3,9600)
S1.angle(60,2000)

S2.angle(60,2000)
ri=[80,30,15,15]
black=(6, 24, -23, 26, 31, -33)
# list=[12,13,14,15,16]
time.sleep(1)
clock = time.clock()
while(True):
    clock.tick()
    img=sensor.snapshot()
    #img.find_blobs([black],roi=ri,x_stride=2,y_stride=2)
    #img.draw_rectangle(ri,color=(1255,0,0))
    #s=img.get_statistics(roi=ri)
    #g=img.find_template(min,0.70,step=4,search=SEARCH_EX)
   # if g:
   #     img.draw_rectangle(g)
   # print(s.a_max(),s.a_min())
   # s='a_max:= {}'.format(s.a_max())+"\n"+'a_min={}'.format(s.a_min())
   # img.draw_string(100,100,s,color=(255,0,0),scale=2)
   # img.draw_line(0,0,300,300,color=(0,255,0),thickness=1)
#    img.draw_circle(200,200,100,color=(128,128,0))
  #  img.draw_cross(100,100,color=(0,128,128))



